package com.websocket;

import com.alibaba.fastjson.JSON;
import com.alibaba.fastjson.JSONObject;
import com.common.converter.FlightRealTimeDataConverter;
import com.entity.FlightData;
import com.entity.FlightRealTimeData;
import com.common.enums.MAV_STATE;
import com.entity.Securityrestriction;
import com.mapper.influx.FlightRealTimeDataMapper;
import com.service.DroneService;
import com.service.ISecurityrestrictionService;
import com.utils.CustomModeHelper;
import com.utils.DroneConnection;
import com.utils.TaskQueueManager;
import io.dronefleet.mavlink.Mavlink2Message;
import io.dronefleet.mavlink.MavlinkConnection;
import io.dronefleet.mavlink.MavlinkMessage;
import io.dronefleet.mavlink.common.*;
import io.dronefleet.mavlink.minimal.Heartbeat;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Component;

import javax.annotation.PostConstruct;
import java.io.IOException;
import java.text.SimpleDateFormat;
import java.time.Duration;
import java.time.Instant;

@Component
public class MavlinkMessageHandler {

    private final MessageWebSocketHandler webSocketHandler;
    private Thread messageHandlingThread;

    // 网络延迟
    private long lastTimeStamp = 0;
    private long networkLatency = 0;
    // 响应时间
//    private long lastTimeStamp = 0;
//    private long networkLatency = 0;

    @Autowired
    private TaskQueueManager taskQueueManager; // 注入任务队列管理器


    @Autowired
    ISecurityrestrictionService securityrestrictionService;

    @Autowired
    DroneService  droneService;
    // 全局变量
    private HomePosition homePosition = null;
    private SysStatus sysStatus = null;

    private FlightData flightData = new FlightData();

    @Autowired
    private FlightRealTimeDataMapper flightRealTimeDataMapper;
    private boolean isRecord = false;
    private String timeId;
    private SimpleDateFormat simpleDateFormat = new SimpleDateFormat("yyyyMMddHHmmss");

    @Autowired
    public MavlinkMessageHandler(MessageWebSocketHandler webSocketHandler) throws IOException {
        this.webSocketHandler = webSocketHandler;
    }

    @PostConstruct
    public void init() throws IOException {

        messageHandlingThread = new Thread(() -> {
            try {
                handleMessages();
            } catch (IOException e) {
                e.printStackTrace();
            }
        });
        messageHandlingThread.start();
    }

    public void handleMessages() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        MavlinkMessage message;
        Thread sendMessageThread = new Thread(() -> {
            try {
                int count = 0;
                while (true){
                    // 将flightData转为JSONObject
                    JSONObject jsonObj = JSON.parseObject(JSON.toJSONString(flightData));

                    Securityrestriction restriction = securityrestrictionService.getById(1);
                    //网络延迟
                    jsonObj.put("latency",networkLatency);

                    //安全防护判断
                    if(flightData.getBattery()<(restriction.getBattery()+0.5)*100){
//                        System.out.println("电量不足");
                        jsonObj.put("batteryRestriction", "1"); // 添加新属性
                    }else if(flightData.getBattery()<restriction.getBattery()*100*1){
//                        System.out.println("电量不足");
                        jsonObj.put("batteryRestriction", "2"); // 添加新属性
                    }//OffBoard
//                    System.out.println("flightData.getX():"+flightData.getX());
//                    System.out.println("flightData.getY():"+flightData.getY());
//                    System.out.println("res.getX():"+restriction.getX());
//                    System.out.println("res.getY():"+restriction.getY());

//                    System.out.println("flightData.getRadius():"+restriction.getRadius());
                    if((flightData.getX()<restriction.getX()- restriction.getRadius()||flightData.getX()>restriction.getX()+restriction.getRadius()
                    ||flightData.getY()<restriction.getY()- restriction.getRadius()||flightData.getY()>restriction.getY()+ restriction.getRadius())){
                        System.out.println("超出电子围栏");
                        if(flightData.getCustomModeText().equals("板外模式")) {
                            jsonObj.put("positionRestriction", "1");
                            droneService.setAutoLoiterMode();
                        }
                    }

                    jsonObj.put("queueIsPaused", taskQueueManager.isPaused());
                    jsonObj.put("queueListCount", taskQueueManager.getPendingTaskCount());

                    jsonObj.put("restrictionRealX",restriction.getX());
                    jsonObj.put("restrictionRealY",restriction.getY());

                    webSocketHandler.sendMessage(jsonObj.toJSONString());
                    if(isRecord){
                        FlightRealTimeData flightRealTimeData
                                = FlightRealTimeDataConverter.INSTANCE.flightDataToFlightRealTimeData(flightData);
                        // 在调用 save 方法之前添加这段代码
//                        System.out.println("flightRealTimeData.time: " + flightRealTimeData.getTime());
//                        System.out.println("flightRealTimeData.timeId: " + flightRealTimeData.getTimeId());
//                        System.out.println("flightRealTimeData.x: " + flightRealTimeData.getX());
//                        System.out.println("flightRealTimeData.y: " + flightRealTimeData.getY());
//                        System.out.println("flightRealTimeData.z: " + flightRealTimeData.getZ());
//                        System.out.println("flightRealTimeData.yaw: " + flightRealTimeData.getYaw());
//                        System.out.println("flightRealTimeData.roll: " + flightRealTimeData.getRoll());
//                        System.out.println("flightRealTimeData.pitch: " + flightRealTimeData.getPitch());
//                        System.out.println("flightRealTimeData.customModeText: " + flightRealTimeData.getCustomModeText());
//                        System.out.println("flightRealTimeData.groundSpeed: " + flightRealTimeData.getGroundSpeed());
//                        System.out.println("flightRealTimeData.systemStatusText: " + flightRealTimeData.getSystemStatusText());
                        if(flightRealTimeData.getGroundSpeed()==null){
                            flightRealTimeData.setGroundSpeed("0");
                        }
                        flightRealTimeData.setTimeId(timeId);
                        if(flightData.getSystemStatusText().equals("未解锁")){
                            isRecord = false;
//                            FlightRealTimeData flightRealTimeData
//                                    = FlightRealTimeDataConverter.INSTANCE.flightDataToFlightRealTimeData(flightData);
//                            flightRealTimeData.setTimeId(timeId);
//                            flightRealTimeDataMapper.save(flightRealTimeData);
//                            count = 0;
                        }
                        else {
                            flightRealTimeDataMapper.save(flightRealTimeData);
//                            FlightRealTimeData flightRealTimeData
//                                    = FlightRealTimeDataConverter.INSTANCE.flightDataToFlightRealTimeData(flightData);
//                            flightRealTimeData.setTimeId(timeId);
//                            if(count % 1 == 0){
//                                flightRealTimeDataMapper.save(flightRealTimeData);
//                            }
//                            count++;
                        }
                    }
                    else{
                        if("解锁".equals(flightData.getSystemStatusText())){
                            isRecord = true;
                            timeId = simpleDateFormat.format(System.currentTimeMillis());
                        }
                    }
                    Thread.sleep(100);
                }
            } catch (Exception e) {
                e.printStackTrace();
            }
        });
        sendMessageThread.start();
        while ((message = connection.next()) != null) {
//            System.out.println("message:"+message);
            if (message instanceof Mavlink2Message) {
                Mavlink2Message message2 = (Mavlink2Message) message;
                if (message2.getPayload() instanceof CommandAck) {
                    CommandAck ack = (CommandAck) message2.getPayload();
                }
            }
            if (message.getPayload() instanceof Heartbeat) {
                // 获取当前UTC时间戳（毫秒）
                long timestampMillis = Instant.now().toEpochMilli();
                networkLatency = timestampMillis - lastTimeStamp;
                lastTimeStamp = timestampMillis;
                MavlinkMessage<Heartbeat> heartbeatMessage = (MavlinkMessage<Heartbeat>) message;
                flightData.setCustomModeText(CustomModeHelper.getCustomModeState(heartbeatMessage.getPayload().customMode()));
                flightData.setSystemStatusText(MAV_STATE.fromValue(heartbeatMessage.getPayload().systemStatus().value()).getDescription());
//                System.out.println("systemStatus:"+heartbeatMessage.getPayload());
            } else if (message.getPayload() instanceof LocalPositionNed) {
                LocalPositionNed localPositionNed = (LocalPositionNed) message.getPayload();
                flightData.setX(localPositionNed.x());
                flightData.setY(localPositionNed.y());
                flightData.setZ(String.format("%.2f", -localPositionNed.z()));
                flightData.setGroundSpeed(String.format("%.2f",Math.sqrt(Math.pow(localPositionNed.vx(),2)+Math.pow(localPositionNed.vy(),2)+Math.pow(localPositionNed.vz(),2))));
//                System.out.println("----------速度:"+flightData.getGroundSpeed());
            } else if (message.getPayload() instanceof HomePosition) {
                homePosition = (HomePosition) message.getPayload();
            } else if (message.getPayload() instanceof SysStatus) {
                sysStatus = (SysStatus) message.getPayload();
                flightData.setBattery(sysStatus.voltageBattery());
//                System.out.println("currentBattery:"+sysStatus.currentBattery());
//                System.out.println("voltageBattery:"+sysStatus.voltageBattery());
//                System.out.println("sysB:"+sysStatus);
            } else if (message.getPayload() instanceof Attitude) {
                Attitude attitude = (Attitude) message.getPayload();
//                System.out.println("-------hhhhhhhhhhhhhhhhhhh："+String.format("%.2f",attitude.yaw()));
                flightData.setYaw(attitude.yaw());
                flightData.setRoll(attitude.roll());
                flightData.setPitch(attitude.pitch());
            } else if (message.getPayload() instanceof VfrHud){
                VfrHud vfrHud = (VfrHud)message.getPayload();
//                System.out.println("速度："+String.format("%.2f",vfrHud.groundspeed()));
//                System.out.println("-------空速："+String.format("%.2f",vfrHud.airspeed()));
                flightData.setGroundSpeed(String.format("%.2f",vfrHud.groundspeed()));
            }
            else if (message.getPayload() instanceof Statustext){
                Statustext statustext = (Statustext)message.getPayload();
//                System.out.println("status："+statustext);
            }
        }
    }

    // 返回homePosition值的接口方法
    public HomePosition getHomePosition() {
        return homePosition;
    }

    public void setTimeId(String timeId) {
        this.timeId = timeId;
    }
}